﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Hont.AStar
{
    public class HontAStar
    {
        IGrid mIGrid;
        Grid mGrid;
        Node mBeginNode;
        Node mTargetNode;
        List<Node> mBuildPathCacheList;
        List<Node> mOpenList;
        SortedList<int, Node> mOpenSortedList;
        /// <summary>
        /// Walked road and closed road.
        /// </summary>
        SortedList<int, Node> mClosedList;
        /// <summary>
        /// 'H' Cost estimate interface.
        /// </summary>
        IAStarHCost mHCost;
        /// <summary>
        /// The straight cost.
        /// </summary>
        int mGStraightCost;
        /// <summary>
        /// The diagonal cost.
        /// </summary>
        int mGDiagonalCost;
        IAStarStepDebuger mDebuger;

        public Grid Grid { get { return mGrid; } }
        public IAStarStepDebuger Debuger { get { return mDebuger; } set { mDebuger = value; } }


        public HontAStar() : this(new EasyAStarHCost())
        {
        }

        public HontAStar(IAStarHCost aStarCost) : this(aStarCost, 14, 10)
        {
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="hCost">'H' Cost estimate interface.</param>
        /// <param name="straightCost">The straight road cost.</param>
        /// <param name="diagonalCost">The diagonal road cost.</param>
        public HontAStar(IAStarHCost hCost, int straightCost, int diagonalCost)
        {
            this.mHCost = hCost;
            this.mGStraightCost = straightCost;
            this.mGDiagonalCost = diagonalCost;

            mDebuger = new EmptyAStarDebuger();
        }

        public void Init(Grid grid)
        {
            this.mGrid = grid;
            this.mIGrid = mGrid;

            mOpenList = new List<Node>(mGrid.Width * mGrid.Height);
            mOpenSortedList = new SortedList<int, Node>(mGrid.Width * mGrid.Height);
            mBuildPathCacheList = new List<Node>();
            mClosedList = new SortedList<int, Node>(mGrid.Width * mGrid.Height);
        }

        /// <summary>
        /// Start pathfinding, if not walkable path will return null.
        /// </summary>
        public Position[] Start(Position startPos, Position endPos, int mask = -1, bool combinePath = true)
        {
            Position[] result = null;

            mDebuger.OnPathfindingBegin(startPos, endPos);

            mOpenList.Clear();
            mOpenSortedList.Clear();
            mClosedList.Clear();
            mBeginNode = mIGrid[startPos.X, startPos.Y, startPos.Z];
            mTargetNode = mIGrid[endPos.X, endPos.Y, endPos.Z];

            //Init first node.
            mBeginNode.Init(0, mHCost.GetCost(mBeginNode.Position, mTargetNode.Position), mBeginNode.Walkable, 1);

            var searchResult = Search(mask);
            if (searchResult)
                result = Nodes2PositionArr(BuildPath());

            if (combinePath)
                result = HontAStarHelper.CombinePath(result);

            return result;
        }

        /// <summary>
        /// Check node is walkable.
        /// </summary>
        bool NodeIsWalkable(Node currentNode, Node compareNode, int mask)
        {
            bool result = false;

            if (compareNode.Walkable
                && compareNode.CheckMask(mask)
                && !mClosedList.ContainsKey(compareNode.ID))
            {
                var adjacentPoint1 = mIGrid[currentNode.X, compareNode.Y, compareNode.Z];
                var adjacentPoint2 = mIGrid[compareNode.X, currentNode.Y, compareNode.Z];
                var adjacentPoint3 = mIGrid[compareNode.X, compareNode.Y, currentNode.Z];

                if (adjacentPoint1.Walkable && adjacentPoint1.CheckMask(mask)
                    && adjacentPoint2.Walkable && adjacentPoint2.CheckMask(mask)
                    && adjacentPoint3.Walkable && adjacentPoint3.CheckMask(mask))
                {
                    if (!mOpenSortedList.ContainsKey(compareNode.ID))
                        result = true;
                }
            }

            return result;
        }

        int GetGValue(Node currentNode, Node compareNode)
        {
            var result = mGDiagonalCost;

            var dirX = compareNode.Position.X - currentNode.Position.X;
            var dirY = compareNode.Position.Y - currentNode.Position.Y;
            var dirZ = compareNode.Position.Z - currentNode.Position.Z;

            if (dirX * dirX + dirY * dirY + dirZ * dirZ == 1)
            {
                result = mGStraightCost;
            }

            result += currentNode.G + compareNode.Cost;

            return result;
        }

        bool Search(int mask)
        {
            var result = true;
            var currentNode = mBeginNode;
            mClosedList.Add(mBeginNode.ID, mBeginNode);
            mDebuger.OnAddToClosedList(mBeginNode.Position);

            while (currentNode != mTargetNode && result)
            {
                var startX = Math.Max(0, currentNode.X - 1);
                var endX = Math.Min(mGrid.Lenght - 1, currentNode.X + 1);
                var startY = Math.Max(0, currentNode.Y - 1);
                var endY = Math.Min(mGrid.Height - 1, currentNode.Y + 1);
                var startZ = Math.Max(0, currentNode.Z - 1);
                var endZ = Math.Min(mGrid.Width - 1, currentNode.Z + 1);

                //Compare around 27 direction.
                for (int i = startX; i <= endX; i++)
                {
                    for (int j = startY; j <= endY; j++)
                    {
                        for (int k = startZ; k <= endZ; k++)
                        {
                            var compareNode = mIGrid[i, j, k];
                            mDebuger.OnToNextPoint(compareNode.Position, DebugerNodeTypeEnum.CompareNode);
                            if (!NodeIsWalkable(currentNode, compareNode, mask)) continue;

                            compareNode.G = GetGValue(currentNode, compareNode);
                            compareNode.H = mHCost.GetCost(compareNode.Position, mTargetNode.Position);
                            compareNode.Parent = currentNode;
                            mOpenList.Add(compareNode);
                            mOpenSortedList.Add(compareNode.ID, compareNode);
                        }
                    }
                }

                result = mOpenList.Count > 0;//Has remain road.

                if (result)
                {
                    /// Sort OpenList and through 'f' value. return first node.
                    mOpenList.Sort((x, y) => x.F.CompareTo(y.F));
                    currentNode = mOpenList[0];
                    mOpenList.Remove(currentNode);
                    mOpenSortedList.Remove(currentNode.ID);
                    mClosedList.Add(currentNode.ID, currentNode);
                    mDebuger.OnAddToClosedList(currentNode.Position);

                    mDebuger.OnToNextPoint(currentNode.Position, DebugerNodeTypeEnum.NextNode);
                }
            }

            return result;
        }

        Node[] BuildPath()
        {
            mBuildPathCacheList.Clear();
            var tNode = mTargetNode;

            mBuildPathCacheList.Add(tNode);

            while (tNode != mBeginNode)
            {
                tNode = tNode.Parent;
                mBuildPathCacheList.Add(tNode);
            }

            mBuildPathCacheList.Reverse();

            return mBuildPathCacheList.ToArray();
        }

        Position[] Nodes2PositionArr(Node[] nodes)
        {
            var result = new Position[nodes.Length];

            for (int i = 0; i < nodes.Length; i++)
            {
                result[i] = new Position(nodes[i].X, nodes[i].Y, nodes[i].Z);
            }

            return result;
        }
    }
}
